package uk.ac.cam.cl.oscar.sift;

import java.util.*;

/**
 * Static class for matching keypoints.
 */
public class KeypointMatcher
{
    /**
     * Finds a set of matches between two sets of keypoints, based
     * on the relative distance between their descriptors.  Due to
     * the existence of EdgeFinder and scaling to match a standard
     * size of 640x480, we've also added relative coordinate tests
     */
    public static Vector<KeypointMatch> findMatches(Vector<Keypoint> kplist1, Vector<Keypoint> kplist2)
    {
        Vector<KeypointMatch> matches = new Vector<KeypointMatch>();
        Vector<Keypoint> working = new Vector<Keypoint>(kplist2);  // keypoints that haven't already been matched

        for (Keypoint kp : kplist1) {
            double distNearest = 1000000.0;
            int nearest = -1;
            double distNearest2 = 1000000.0;
            int nearest2 = -1;

            /* Searches for a match which has a distance no greater than 60% of the next largest */
            for (int i=0; i<working.size(); i++) {
                Keypoint kp2 = working.elementAt(i);

                double dist = keypointDist(kp, kp2);
                if (dist < distNearest) {
                    // (Math.abs(kp.getX() - kp2.getX()) < 20) && 
                      if ((Math.abs(kp.getY() - kp2.getY()) < 10)) {
                        nearest2 = nearest;
                        distNearest2 = distNearest;

                        nearest = i;
                        distNearest = dist;
                      }
                }
            }

            if ((nearest != -1) && (nearest2 != -1)) {
                if ((distNearest / distNearest2) < 0.7) {
                    matches.add(new KeypointMatch(kp, working.elementAt(nearest), distNearest));
                    working.removeElementAt(nearest);
                }
            }
        }
        
        return matches;
    }

    // Calculates the relative distance between the keypoint descriptors
    public static double keypointDist(Keypoint a, Keypoint b)
    {
        double distance = 0.0;
        KeypointDescriptor kpda = a.getDescriptor();
        KeypointDescriptor kpdb = b.getDescriptor();

        for (int i=0; i<kpda.getLinearDim(); i++) {
            double dimDist = kpda.linearGet(i) - kpdb.linearGet(i);
            distance += dimDist * dimDist;
        }
        return distance;
    }
} 
